/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "rtthread.h"
#include "uart.h"
#include "camera_rc.h"
#include "rgb.h"
#include "dev_info.h"
#include "combos.h"

camera_data_t camera_rc_data;
// uint8_t camera_rc_buffer[CAMERA_RC_COUNT];
void camera_rc_send_back_update(void);
bool is_camera_combos_trig(void)
{
    return (camera_rc_data.flag & (1 << CAMERA_COMBOS_MODE)) != 0;
}
bool is_camera_unlock_trig(void)
{
    return (camera_rc_data.flag & (1 << CAMERA_UNLOCK)) != 0;
}
bool is_camera_lock_trig(void)
{
    return (camera_rc_data.flag & (1 << CAMERA_LOCK)) != 0;
}
int8_t camera_data_decode(uint8_t *DataFlow, uint8_t len)
{
    if (len < 64)
        return -1; // 数据长度不合法
    if ((DataFlow[0] == CAMERA_RC_HEADER) && (DataFlow[63] == CAMERA_RC_TAIL))
    {
        uint8_t tempCheck = DataFlow[1];
        for (uint8_t i = 2; i <= 61; i++)
        {
            tempCheck ^= DataFlow[i];
        }
        tempCheck = tempCheck & 0xff;
        // rt_kprintf("check: c-%02X r-%02X\n", tempCheck, DataFlow[62]);
        if (tempCheck == DataFlow[62])
        {
            // camera_rc_buffer[CAMERA_CHECK] = tempCheck;
            // camera_rc_buffer[CAMERA_ROLL] = DataFlow[7];
            // camera_rc_buffer[CAMERA_PITCH] = DataFlow[8];
            // camera_rc_buffer[CAMERA_THROTTLE] = DataFlow[9];
            // camera_rc_buffer[CAMERA_YAW] = DataFlow[10];
            // camera_rc_buffer[CAMREA_FLAG_ONE] = DataFlow[15];
            // camera_rc_buffer[CAMREA_FLAG_TWO] = DataFlow[16];
            // camera_rc_buffer[CAMREA_FLAG_THREE] = DataFlow[17];
            // camera_rc_buffer[CAMREA_FLAG_FOUR] = DataFlow[18];

            camera_rc_data.roll = DataFlow[7];
            camera_rc_data.pitch = DataFlow[8];
            camera_rc_data.throttle = DataFlow[9];
            camera_rc_data.yaw = DataFlow[10];
            camera_rc_data.flag = DataFlow[15] | (DataFlow[16] << 8) | (DataFlow[17] << 16) | (DataFlow[18] << 24);

            camera_rc_data.arm_bucket = DataFlow[CAMERA_ARM_BUCKET];
            camera_rc_data.arm_big = DataFlow[CAMERA_ARM_BIG];
            camera_rc_data.arm_small = DataFlow[CAMERA_ARM_SMALL];
            camera_rc_data.arm_rotate = DataFlow[CAMERA_ARM_ROTATE];

            if (camera_rc_data.flag & (1 << CAMERA_COMBOS_MODE))
            {
                int16_t throttle = DataFlow[44] << 8 | DataFlow[45];
                int16_t turn = DataFlow[46] << 8 | DataFlow[47];
                int16_t shift = DataFlow[48] << 8 | DataFlow[49];
                int16_t turrentPitch = DataFlow[50] << 8 | DataFlow[51];
                uint16_t fire = DataFlow[52] << 8 | DataFlow[53];
                uint16_t moveTime = DataFlow[54] << 8 | DataFlow[55];
                uint16_t fireTime = DataFlow[56] << 8 | DataFlow[57];
                uint8_t cmdIndex = DataFlow[58];
                uint8_t cmdCount = DataFlow[59];
                set_combos_0_cmd(cmdIndex, cmdCount, throttle, turn, shift, turrentPitch, fire, moveTime, fireTime);
                // rt_kprintf("CAMERA_COMBOS_MODE\n");
            }

            // rt_kprintf("flag:%08X index:%d\n", camera_rc_data.flag, cmdIndex);
        }
        else
        {
            // LostCnt++;
            return -2;
        }
    }
    return 0;
}

void camera_task(void *parameter)
{
    uint16_t rssi = 0;
    uint32_t dt = 0;
    while (1)
    {
        if (wait_for_uart1_rx_done_forever())
        {

            uint16_t len = uart1_rxLen();
            uint8_t *rxBuff = uart1_rxBuff();
            rt_kprintf("\n uartBuffer len:%d -> \n", len);
            for (int i = 0; i < len; i++)
            {
                if (i != 0 && i % 16 == 0)
                    rt_kprintf("\n");
                rt_kprintf("%02X ", rxBuff[i]);
            }
            rt_kprintf("\n\n");
            // uart2_put(rxBuff+44, len>44?len-44:len);
            if (camera_data_decode(rxBuff, len) == 0)
            {
                rssi++;
                if (rt_tick_get() > (dt + 1000))
                {
                    dt = rt_tick_get();
                    camera_rc_data.rssi = rssi;
                    rssi = 0;
                    // rt_kprintf("camera rc rssi:%d\n", camera_rc_data.rssi);
                }
            }
            else
            {
                // rt_kprintf("camera crc error\n");
            }
            // camera_rc_send_back_update();
        }
        else
        {
            camera_rc_data.rssi = 0;
            // rt_kprintf("camera uart rx timeout\n");
        }
    }
}
void camera_rc_send_back_update(void)
{
    uint8_t camera_send_back_data[64];
    for (int i = 0; i < 32; i++)
    {
        camera_send_back_data[i] = 0;
    }
    uint32_t sys_state = get_rgb_state();
    uint8_t data3 = (sys_state >> 24) & 0xff;
    uint8_t data2 = (sys_state >> 16) & 0xff;
    uint8_t data1 = (sys_state >> 8) & 0xff;
    uint8_t data0 = sys_state & 0Xff;

    camera_send_back_data[CAMERA_BACK_HEADER] = 0x66;
    camera_send_back_data[CAMERA_BACK_TYPE] = 0x01;      // 默认使用1，表示回传
    camera_send_back_data[CAMERA_BACK_DATA_TYPE] = 0x00; // 暂时没有用， 数据太多时，不够放，可以用个值表示不同组的数据发送
    camera_send_back_data[CAMERA_BACK_MAJOR_VER] = 3;
    camera_send_back_data[CAMERA_BACK_MINOR_VER] = 5;
    camera_send_back_data[CAMERA_BACK_BUILD_NUMBER] = 3;
    camera_send_back_data[CAMERA_BACK_CORE_ID] = 8; // 识别为ZINOPREMIUM //get_dev_pid()>>16;
    camera_send_back_data[CAMERA_BACK_DEV_ID] = 1;  // 识别为Mecha //get_dev_pid()>>8;
    // #ifdef HeartOfMecha
    //     camera_send_back_data[CAMERA_BACK_EQM_ID] = getTransformState() + 10; // 第一个型号从10开始
    // #else
    //     camera_send_back_data[CAMERA_BACK_EQM_ID] = 1;//识别为水弹炮 //get_dev_pid();
    // #endif
    camera_send_back_data[CAMERA_BACK_EQM_ID] = 11; // 识别为水弹炮 //get_dev_pid();
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_3] = data3;
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_2] = data2;
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_1] = data1;
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_0] = data0;
    camera_send_back_data[CAMERA_BACK_TAIL] = 0x99;

    uint8_t tempCheck = camera_send_back_data[1];
    for (uint8_t i = 2; i <= 29; i++)
    {
        tempCheck ^= camera_send_back_data[i];
    }
    tempCheck = tempCheck & 0xff;
    camera_send_back_data[CAMERA_BACK_CHECK] = tempCheck;
    camera_send_back_data[0] = 0x66;
    camera_send_back_data[31] = 0x99;

    uart1_put(camera_send_back_data, 32);
}

ALIGN(8)
static rt_uint8_t camera_stack[512];
struct rt_thread camera_thread;
int camera_task_init(void)
{
    rt_thread_t tid = &camera_thread;
    rt_err_t result = rt_thread_init(tid, "camera", camera_task, RT_NULL,
                                     camera_stack, sizeof(camera_stack), 10, 20);
    if (result == RT_EOK)
    {
        rt_thread_startup(tid);
    }
    else
    {
        rt_kprintf("camera thread init failed\n");
        return -1;
    }
    return 0;
}
// INIT_APP_EXPORT(camera_task_init);
